﻿using Microsoft.Kinect;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace PewPew
{
    class QrPoint
    {
        public float X;
        public float Y;
        public float Z;

        private const int QR_POINT_HISTORY_LENGTH = 12;
        private const int QR_POINT_SQUARES = 4;
        public static QrPoint[][] QrPointsHistory = new QrPoint[QR_POINT_HISTORY_LENGTH][];

        public static QrPoint[] CreateFromZXingArray(ZXing.ResultPoint[] oldPoints, int maxWidth, int maxHeight)
        {
            QrPoint[] points = new QrPoint[oldPoints.Length];

            for (var i = 0; i < oldPoints.Length; i++)
            {
                if (oldPoints[i].X < maxWidth && oldPoints[i].Y < maxHeight)
                {
                    points[i] = new QrPoint { X = oldPoints[i].X, Y = oldPoints[i].Y };
                }
                else
                {
                    points[i] = null;
                }
            }

            return points;
        }

        public void FixZByDepthPixels(DepthImagePixel[] depthPixels, int width)
        {
            var x = (int)this.X;
            var y = (int)this.Y;
            this.Z = depthPixels[width * ((480*2 - y) / 2) + (x / 2)].Depth >> DepthImageFrame.PlayerIndexBitmaskWidth;; //TODO so much magic
        }

        public static QrPoint AveragePoint(QrPoint[] points)
        {
            QrPoint avg = new QrPoint();
            int count = 0;
            foreach (QrPoint p in points)
            {
                if (p != null)
                {
                    avg.X += p.X;
                    avg.Y += p.Y;
                    count++;
                }
            }

            avg.X /= count;
            avg.Y /= count;
            return avg;
        }

        public static Boolean LegalPointSet(QrPoint[] points)
        {
            for (int i = 0; i < points.Length; i++)
            {
                if (points[i] == null)
                {
                    return false;
                }
            }

            return true;
        }

        public static void UpdateHistory(QrPoint[] newPoint)
        {
            for (int i = QrPointsHistory.Length - 1; i > 0; i--)
            {
                QrPointsHistory[i] = QrPointsHistory[i - 1];
            }
            QrPointsHistory[0] = newPoint;
        }

        public static QrPoint[] FixPointsByAverage(QrPoint[] pointToFix)
        {
            QrPoint[] result = new QrPoint[pointToFix.Length];
            



            return result;
        }

        private static QrPoint[] CalculateAverage(QrPoint[][] pointsToCalc, float weight)
        {
            QrPoint[] average = new QrPoint[QR_POINT_SQUARES];
            var currWeight = weight;
            var endWeight = 2f - weight;

            var diffWeight = (float)(1 / pointsToCalc.Length);

            for (int i = 0; i < pointsToCalc.Length; i++)
            {
                for (int j = 0; j < average.Length; j++)
                {
                    if (average[j] == null)
                    {
                        average[j] = new QrPoint();
                    }

                    average[j].X += (pointsToCalc[i][j].X * currWeight);
                    average[j].Y += (pointsToCalc[i][j].Y * currWeight);
                    average[j].Z += (pointsToCalc[i][j].Z * currWeight);

                    currWeight -= diffWeight;
                }
            }

            for (int i = 0; i < average.Length; i++)
            {
                average[i].X /= pointsToCalc.Length;
                average[i].Y /= pointsToCalc.Length;
                average[i].Z /= pointsToCalc.Length;
            }

            return average;
        }

        private static QrPoint[] CalculateAverageWithWeight(QrPoint[][] pointsToCalc)
        {
            return CalculateAverage(pointsToCalc, 1.5f);
        }

        private static QrPoint[] CalculateAverageWithoutWeight(QrPoint[][] pointsToCalc)
        {
            return CalculateAverage(pointsToCalc, 1f);
        }
    }
}
